
// RTOS Kernel Headers
#include <task.h>
#include <kernelConfig.h>
#include <interrupt.h>
#include <kernelAsm.h>

// Table for the hierarchical bit map algorithm
unsigned char mapTable[256] =
{
		0, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4,
		5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
		6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
		6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
		8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
};

// Bitmap variables
unsigned char L0[32];
unsigned char L1[4];
unsigned char L2;

struct TCB idleTask;	// Task's TCB(Task Control Block) pointer to the idle task
unsigned char idleStack[CONFIG_TASK_STACK_SIZE];	// Stack of the idle task

struct TASK_QUEUE readyList[CONFIG_MAX_PRIORITY];	// List the Ready tasks
struct TASK_QUEUE waitList[CONFIG_EVENT_FLAG_SIZE];	// List the Wait tasks

/*
 * Name	 : InitTask
 * Description : Create the idle task.
 * Parameter : None
 */
int InitTask(void)
{
	struct TCB *task = &idleTask;

	task->prio = 0;
	task->basePrio = 0;

	task->state = TASK_STATE_READY;

	task->stackAddr = idleStack;
	task->stackPoint = 0;
//	task->stackPoint = idleStack + CONFIG_TASK_STACK_SIZE;
	task->stackSize = CONFIG_TASK_STACK_SIZE;

	task->taskFunction = 0;
	task->args = 0;

	task->id = (unsigned int)task;

//	TaskInitContext(task);

	TaskEnqueue(task);

	currentTask = task;

	return 0;
}

/*
 * Name	 : TaskCreate
 * Description : Create the task and initialize.
 *               Created task is inserted to the ready-list and immediately scheduled by scheduler.
 * Parameter : task - Pointer to the task's TCB(Task Control Block) structure
 *             taskFunction - Pointer to the task entry function
 *             args - Pointer that will be used as the parameter for the task being created
 *             prio - Priority that the task should run
 *             stackAddr - Pointer of the task's top of stack
 *             stackSize - Size of the task stack
 */
int TaskCreate(struct TCB *task, int (*taskFunction)(void *), void *args, unsigned int prio, unsigned char* stackAddr, unsigned int stackSize)
{
//	unsigned int flag;

	task->prio = prio;
	task->basePrio = prio;

	task->state = TASK_STATE_READY;

	task->stackAddr = stackAddr;
	task->stackPoint = task->stackAddr + stackSize - sizeof(struct CONTEXT_FRAME);
	task->stackSize = stackSize;

	task->taskFunction = taskFunction;
	task->args = args;

	task->id = (unsigned int)task;

	task->timeQuantum = CONFIG_SCHED_TIME_QUANTUM;

	TaskInitContext(task);

//	flag = IntSaveDisableIRQ();
	DISABLE_INTERRUPT();

	TaskEnqueue(task);

//	DoScheduling();

//	IntRestoreIRQ(flag);
	ENABLE_INTERRUPT();

	return 0;
}

/*
 * Name	 : TaskInitContext
 * Description : Initialize context.
 * Parameter : task - Pointer to the task's TCB(Task Control Block) structure
 */
void TaskInitContext(struct TCB *task)
{
	struct CONTEXT_FRAME* ctx = (struct CONTEXT_FRAME*)(task->stackPoint);


	ctx->pc = (unsigned int)task->taskFunction;
	ctx->r0 = (unsigned long)task->args;
	ctx->r1 = 1;
	ctx->r2 = 2;
	ctx->r3 = 3;
	ctx->r4 = 4;
	ctx->r5 = 5;
	ctx->r6 = 6;
	ctx->r7 = 7;
	ctx->r8 = 8;
	ctx->r9 = 9;
	ctx->r10 = 10;
	ctx->r11 = 11;
	ctx->r12 = 12;
	ctx->r13 = (unsigned long)task->stackPoint;
	ctx->lr = (unsigned long)TaskExit;
	ctx->cpsr = 0x3f;
}

/*
 * Name	 : TaskExit
 * Description : Destroy the task.
 *               Destroyed task is removed from the ready-list and immediately scheduled by scheduler.
 * Parameter : None
 */
int TaskExit(void)
{
//	unsigned int flag;

//	flag = IntSaveDisableIRQ();
	DISABLE_INTERRUPT();

	TaskDequeue(currentTask);

	currentTask->id = 0;

	DoScheduling();

//	IntRestoreIRQ(flag);
	ENABLE_INTERRUPT();

	return 0;
}

/*
 * Name	 : TaskGetId
 * Description : Obtain the task ID that running state.
 * Parameter : None
 */
int TaskGetId(void)
{
	return currentTask->id;
}

/*
 * Name	 : TaskYield
 * Description : Occur the context switch
 * Parameter : None
 */
int TaskYield(void)
{
	TaskDequeue(currentTask);
	TaskEnqueue(currentTask);

	DoScheduling();

	return 0;
}

/*
 * Name	 : TaskGetPriority
 * Description : Obtain the priority of task that running state.
 * Parameter : None
 */
int TaskGetPriority(void)
{
	return currentTask->prio;
}

/*
 * Name	 : TaskSetPriority
 * Description : Set the priority of task that running state
 * Parameter : prio - Priority that the task will be set
 */
void TaskSetPriority(int prio)
{
//	unsigned int flag;

//	flag = IntSaveDisableIRQ();
	DISABLE_INTERRUPT();

	TaskDequeue(currentTask);

	currentTask->prio = prio;

	TaskEnqueue(currentTask);

//	IntRestoreIRQ(flag);
	ENABLE_INTERRUPT();
}

/*
 * Name	 : TaskPriorityInherit
 * Description : Set the priority of task that owns the mutex to Mutex's Ceiling priority
 * Parameter : mutexOwner - Pointer of the task owns the mutex
 *             newPrio - Priority that the task will be set (Mutex's Ceiling priority)
 */
void TaskPriorityInherit(struct TCB *mutexOwner, unsigned int newPrio)
{
	if(currentTask->prio > mutexOwner->prio)
	{
		mutexOwner->prio = newPrio;
	}
}

/*
 * Name	 : TaskPriorityDisinherit
 * Description : Set the priority of task that owns the mutex to task's base priority
 * Parameter : None
 */
void TaskPriorityDisinherit(struct TCB *mutexOwner)
{
	if(mutexOwner->prio != mutexOwner->basePrio)
	{
		mutexOwner->prio = mutexOwner->basePrio;
	}
}

/*
 * Name	 : InitScheduler
 * Description : Initialize ready-list
 * Parameter : None
 */
void InitScheduler(void)
{
	int i;

	for(i = 0; i < CONFIG_MAX_PRIORITY; i++)
	{
		readyList[i].next = (struct TCB*)&readyList[i];
		readyList[i].prev = (struct TCB*)&readyList[i];
	}
}

/*
 * Name	 : TaskEnqueue
 * Description : Insert the task's TCB pointer to ready-list
 * Parameter : task - Pointer to the task's TCB(Task Control Block) structure that will be insert into ready-list
 */
int TaskEnqueue(struct TCB* task)
{
	if(readyList[task->prio].next == (struct TCB*)&readyList[task->prio])
	{
		readyList[task->prio].next = task;

		task->prev = task;
		task->next = task;

		SetBitmapIndex(task->prio);
	}
	else
	{
		task->next = readyList[task->prio].next;
		task->prev = readyList[task->prio].next->prev;

		task->prev->next = task;
		readyList[task->prio].next->prev = task;
	}

	return 0;
}

/*
 * Name	 : TaskDequeue
 * Description : Remove the TCB pointer from ready-list
 * Parameter : task - Pointer to the TCB(Task Control Block) structure that will be remove from ready-list
 */
int TaskDequeue(struct TCB* task)
{
	if(readyList[task->prio].next == readyList[task->prio].next->next)
	{
		readyList[task->prio].next = (struct TCB *)&readyList[task->prio];
		readyList[task->prio].prev = (struct TCB *)&readyList[task->prio];

		ClearBitmapIndex(task->prio);
	}
	else
	{
		readyList[task->prio].next = task->next;

		task->prev->next = task->next;
		task->next->prev = task->prev;
	}

	return 0;
}

/*
 * Name	 : SetBitmapIndex
 * Description : Set the bitmap index
 * Parameter : prio - Priority that the task will be set
 */
int SetBitmapIndex(unsigned int prio)
{
	L0[prio >> 3] = L0[prio >> 3] | (0x01 << (prio & 0x07));
	L1[prio >> 6] = L1[prio >> 6] | (0x01 << ((prio >> 3) & 0x07));
	L2 = (L2 | (0x01 << ((prio >> 6) & 0x07))) & 0x0F;

	return 0;
}

/*
 * Name	 : ClearBitmapIndex
 * Description : Clear the bitmap index
 * Parameter : prio - Priority that the task will be clear
 */
int ClearBitmapIndex(unsigned int prio)
{
	L0[prio >> 3] = L0[prio >> 3] & ~(0x01 << (prio & 0x07));

	if(!L0[prio >> 3])
	{
		L1[prio >> 6] = L1[prio >> 6] & ~(0x01 << ((prio >> 3) & 0x07));

		if(!L1[prio >> 6])
		{
			L2 = L2 & ~(0x01 << ((prio >> 6) & 0x07));
		}
	}

	return 0;
}

/*
 * Name	 : DoScheduling
 * Description : Forcing a context switch
 * Parameter : None
 */
void DoScheduling(void)
{
	struct TCB* current;
	struct TCB* next;

	current = currentTask;
	next = TaskSelect();

	if(next == current)
	{
		return;
	}

	YIELD();
}

/*
 * Name	 : TaskSelect
 * Description :
 * Parameter : None
 */
struct TCB* TaskSelect(void)
{
	struct TCB *task = &idleTask;

	unsigned char indexL2, indexL1, indexL0;

	unsigned char highPriority = 0;

	indexL2 = mapTable[L2] - 1;
	indexL1 = mapTable[L1[indexL2]] - 1;
	indexL0 = mapTable[L0[(indexL2 << 3) + indexL1]] - 1;

	highPriority = (indexL2 << 6) + (indexL1 << 3) + indexL0;

	if(readyList[highPriority].next != (struct TCB *)&readyList[highPriority])
	{
		task = readyList[highPriority].next;
	}

	return task;
}

// 현재 태스크의 Context를 저장하고 다음에 실행할 태스크의 Context로 바꾸는 함수
/*
 * Name	 : ContextSwitch
 * Description : Save the context that task of running state and Restore the context that will be changed task
 * Parameter : currentTask - Pointer to the task's TCB(Task Control Block) structure that task of running state
 *             nextTask - Pointer to the task's TCB(Task Control Block) structure that will be change to running state
 */
void ContextSwitch()
{
	currentTask = TaskSelect();
}

/*
 * Name	 : TaskEventRegister
 * Description : Insert the task's TCB pointer to event-list
 *               Make wait task until specific event occurred
 * Parameter : eventNum - Event number
 */
void TaskEventRegister(unsigned int eventNum)
{
	/* fixme */
//	TaskWait(waitList[eventNum], currentTask);
}

/*
 * Name	 : TaskEventNotify
 * Description : Set the event flag bit to notify that specific event occurred
 *               Make wake-up all task that waited until specific event occurred
 * Parameter : eventNum - Event number
 */
void TaskEventNotify(unsigned int eventNum)
{
	/* fixme */
//	TaskWakeupAll(waitList[eventNum]);
}

/*
 * Name	 : TaskWait
 * Description : Insert the task's TCB pointer to any wait-list
 *               Inserted task is removed from ready-list
 * Parameter : waitList - Wait-list the any resource
 *             task - Pointer to the task's TCB(Task Control Block) structure that will be insert to specific wait-list
 */
int TaskWait(struct TASK_QUEUE *waitList, struct TCB* task)
{
	/* fixme */
//	TaskDequeue(task);
//
//	task->state = TASK_STATE_WAITING;
//
//	// Sorted List濡�蹂�꼍�댁빞 ��//	task->next = waitList->next;
//	task->prev = waitList;
//
//	waitList->prev->next = task;
//	waitList->prev = task;
//
//	DoScheduling();

	return 0;
}

/*
 * Name	 : TaskWakeup
 * Description : Wake up the task that waited in any wait-list
 * Parameter : waitList - Wait-list the any resource
 *             task - Pointer to the TCB(Task Control Block) structure that will be insert to ready-list
 */
int TaskWakeup(struct TASK_QUEUE *waitList, struct TCB* task)
{
	/* fixme */
//	task->prev->next = task->next;
//	task->next->prev = task->prev;
//
//	if(waitList->next == task)
//	{
//		waitList->next = (struct TCB *)&waitList;
//		waitList->prev = (struct TCB *)&waitList;
//	}
//
//	TaskEnqueue(task);
//
//	DoScheduling();

	return 0;
}

/*
 * Name	 : TaskWakeupAll
 * Description : Wake up the all tasks that waited in any wait-list
 * Parameter : waitList - Wait-list the any resource
 */
int TaskWakeupAll(struct TASK_QUEUE *waitList)
{
	/* fixme */
//	struct TCB *task;
//
//	task = waitList->next;
//
//	while(1)
//	{
//		if(task == (struct TCB *)&waitList)
//		{
//			break;
//		}
//
//		TaskEnqueue(task);
//
//		task = task->next;
//	}
//
//	waitList->next = (struct TCB *)&waitList;
//	waitList->prev = (struct TCB *)&waitList;

	return 0;
}




